// Feature_Pitch.cpp: implementation of the CFeature_Pitch class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "LXH_Feature_Pitch.h"
#include "OptionSetting.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CFeature_Pitch::CFeature_Pitch()
{
	para.PWinSz = 0;//0
	para.DownCoef = 3;
	para.CutCoef = 0.3f;
	//para.MinPit = 50;//30;
	//para.MaxPit = 500;//300
	para.MinPit = OptionSetting.para_Option_setting.MinPit;
	para.MaxPit = OptionSetting.para_Option_setting.MaxPit;
	para.Smooth = 1;
}

CFeature_Pitch::~CFeature_Pitch()
{

}

CData* CFeature_Pitch::ExtractFeature()
{
	if( fSignal == NULL )
		ErrorMsg("You must load signal before : CopySignal");
	if( para.PWinSz == 0 ) 
		para.PWinSz = para.SRate / para.MinPit;

	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	if(NumWin <= 0) return new CData(0,0);
	int NumPWin = (nLen - para.PWinSz) / para.StpSz + 1;
	if(NumPWin <= 0) return new CData(0,0);
	CData *pitch;
	if( NumWin > NumPWin ) pitch = new CData( NumWin, 1 );
	else pitch = new CData( NumPWin, 1 );

	float fZero, fMean = 0;
	int i;
	for( i = 0; i < NumPWin; i++ )
	{
		pitch->fMat[i][0] = AutoCor( fSignal + i*para.StpSz, para.PWinSz );
		fMean += pitch->fMat[i][0];
	}
	fMean /= pitch->nRow;
	fZero = fMean / 20;

	i = NumPWin;
	while( i < NumWin )
		pitch->fMat[i++][0] = pitch->fMat[NumPWin-1][0];


	if( para.Smooth )
	{
#define NUMBEROFPULERROR 1
		//Eliminate pulse erreur
		//You can clear this segment
		int nStart;
		int nCount;
		BOOL bZero;

		//Eliminate single 0 Pulse
		bZero = FALSE;
		nStart = 0;
		nCount = 0;
		while( nStart < NumWin)
		{
			if( (pitch->fMat[nStart][0] < fZero ) && (!bZero))
			{
				bZero = TRUE;
				nCount = 1;
			}
			else
			{
				if(( pitch->fMat[nStart][0] < fZero )&&(bZero)) nCount++;
				else 
				{
					if(( bZero )&&( nCount<= NUMBEROFPULERROR ))
					{
						for(int t = 1; t <= nCount; t++)
						{
							float newF = 0;
							if( nStart - t == 0 )
								pitch->fMat[nStart-t][0] = 0;
							else
							{
								newF += pitch->fMat[nStart-t-1][0];
								if( nStart - t + 1 < pitch->nRow )
									newF = (newF + pitch->fMat[nStart-t+1][0])/2;
								pitch->fMat[nStart-t][0] = newF;
							}
						}
					}
					nCount = 0;
					bZero = FALSE;
				}
			}
			nStart++;
		}

		nCount = 0;
		nStart = 0;
		bZero = TRUE;
		while( nStart < NumWin)
		{
			if( (pitch->fMat[nStart][0] > fZero ) && (bZero))
			{
				bZero = FALSE;
				nCount = 1;
			}
			else
			{
				if(( pitch->fMat[nStart][0] > fZero )&&(!bZero)) nCount++;
				else 
				{
					if(( !bZero )&&( nCount<= NUMBEROFPULERROR ))
					{
						for(int t = 1; t <= nCount; t++)
							pitch->fMat[nStart-t][0] = 0;
					}
					nCount = 0;
					bZero = TRUE;
				}
			}
			nStart++;
		}

		fMean = 0;
		int FNumber = 0;
		for( i = 0; i < pitch->nRow; i++ )
		{
			if( pitch->fMat[i][0] > fZero ) 
			{
				fMean += pitch->fMat[i][0];
				FNumber++;
			}
		}
		fMean /= FNumber;

#define ERRORCOEF 3.0f/2
		if (pitch->fMat[0][0] > ERRORCOEF*fMean) pitch->fMat[0][0] = fMean;
		for( i = 1; i < pitch->nRow; i++ )
		{
			if (pitch->fMat[i][0] > ERRORCOEF*fMean) pitch->fMat[i][0] = pitch->fMat[i-1][0];
		}
#undef ERRORCOEF

		//float temp;
		//for( i = 1; i < pitch->nRow - 1; i++ )
		//{
			//temp = (pitch->fMat[i-1][0] + pitch->fMat[i+1][0])/2;
			//if(  (pitch->fMat[i-1][0] > fZero) && (pitch->fMat[i+1][0] > fZero) && (pitch->fMat[i][0] - temp > pitch->fMat[i][0]/5 ))
			//{
				//pitch->fMat[i][0] = temp;
			//}
		//}

	
	//You can clear this segment
	//Eliminate pulse erreur

	//Eliminate segments Fo = 0 
	/*
		nStart = 0;
		while((nStart<pitch->nRow)&&( pitch->fMat[nStart][0] > fZero))nStart++;
		if(nStart == pitch->nRow) return pitch;
		else
			for(int i = nStart-1; i >=0; i--)
				pitch->fMat[i][0] = pitch->fMat[nStart][0];

		for(int i = nStart+1; i < pitch->nRow ; i++)
			if(pitch->fMat[i][0]<fZero){
				pitch->fMat[i][0] = pitch->fMat[i-1][0];
			}
	*/
	
#undef NUMBEROFPULERROR
	}
	return pitch;
}

float CFeature_Pitch::AutoCor(float *fSignal, int nLen)
{
	float temp; //tim max
	float max = 0;
	int   nDownLen = (nLen - 1)/para.DownCoef+1;
	short *frameData = new short [nDownLen];
	float *SSig = new float [nDownLen];
	int i,j;
	
	//CLIPPING
	max = 0;
	j = 0;
	for(i=0; i< nLen; i+=para.DownCoef)
	{
		temp = float(fabs(fSignal[i]));
		temp = temp * temp;
		SSig[j] = temp;
		if( fSignal[i] < 0 ) 
		   SSig[j] = -SSig[j];
		if(max < temp) 
		{
			max = temp;
		}
		j++;
	}
	
	//3 level center clipping
	float thres = para.CutCoef * max;
	j=0;
	for(j=0,i=0 ; i< nLen; j++, i+=para.DownCoef)
	{
		if( SSig[j] > thres)
			frameData[j] = 128;
			//frameData[i] = SSig[j];
		else if(SSig[j] < -thres) 
				frameData[j] = -128;
				//frameData[i] = SSig[j];
			 else frameData[j] = 0;
	}

	//AUTOCORELATION
	//ASSERT(nLen < 500);
	LONG *r = (LONG *)calloc(nDownLen, sizeof(LONG));

	//pitch change supression
	int maxi, mini; 
	maxi = para.SRate / (para.MinPit*para.DownCoef);
	mini = para.SRate / (para.MaxPit*para.DownCoef);

	if(mini+1 > nDownLen) mini = nDownLen - 1;
	if(maxi+1 > nDownLen) maxi = nDownLen - 1;

	for (i=0; i< nDownLen; i++)
		r[0]+= LONG(frameData[i]*frameData[i]);

	for (i= mini;i<maxi; i++){
		for (j=0;j<nDownLen-i;j++){
			if(frameData[j] & 128)
				r[i] += ( frameData[j] & 0x8000) ? -((frameData[j+i])<<7) : ((frameData[j+i])<<7);
			//r[i] += frameData[j+i] * frameData[j];
		}
	}

	//PITCH ESTIMATE
	LONG pos[2]; 
	LONG peak[2];

	//initialize
	for(i=0;i<2;i++) 
	{
		pos[i] = -1;
		peak[i] = 0;
	}

	for(i= mini+1; i < maxi-1; i++)
		if(r[i] > r[i-1] && r[i] >= r[i+1]){
			j = peak[0] > peak[1] ? 1 : 0;
			//get two candidate
			if(r[i] > peak[j]){
				pos[j] = i;
				peak[j] = r[i];
			}
		}
	
	//pitch period halving/doubling
	i = peak[0] > peak[1] ? 1 : 0;
	j = 1-i; //index of max
	
	float kq;
	if(peak[j] < 0.3*r[0]) 
		kq = 0; //unvoice
	else 
		if(pos[i] != -1){
			int m,n;
			m = pos[0] > pos[1] ? 1 : 0;
			n = 1-m;
			
			if(abs(pos[n] / 2 - pos[m]) < int(0.00014*para.SRate)
			   && peak[j] - peak[i] < 5) //??
			{	
			   kq = float(para.SRate / pos[n]);
			}
			else kq = float(para.SRate / pos[j]);
		}
		else 	
			kq = float(para.SRate / pos[j]);
	
	delete[] frameData;
	free(r);
	delete[] SSig;
	return kq/para.DownCoef;
}
